﻿using Microsoft.Xna.Framework;
using System;
using System.Runtime.CompilerServices;

namespace Meow.Core;

public static class XnaMathExts
{
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void ExtractRotation(this Matrix matrix, ref Quaternion rotation)
    {
        if (float.IsNaN(matrix.M11))
            return;

        var sx = (Math.Sign(matrix.M11 * matrix.M12 * matrix.M13 * matrix.M14) < 0) ? -1.0f : 1.0f;
        var sy = (Math.Sign(matrix.M21 * matrix.M22 * matrix.M23 * matrix.M24) < 0) ? -1.0f : 1.0f;
        var sz = (Math.Sign(matrix.M31 * matrix.M32 * matrix.M33 * matrix.M34) < 0) ? -1.0f : 1.0f;

        sx *= (float)Math.Sqrt(matrix.M11 * matrix.M11 + matrix.M12 * matrix.M12 + matrix.M13 * matrix.M13);
        sy *= (float)Math.Sqrt(matrix.M21 * matrix.M21 + matrix.M22 * matrix.M22 + matrix.M23 * matrix.M23);
        sz *= (float)Math.Sqrt(matrix.M31 * matrix.M31 + matrix.M32 * matrix.M32 + matrix.M33 * matrix.M33);

        if (sx == 0.0 || sy == 0.0 || sz == 0.0)
        {
            rotation = Quaternion.Identity;
            return;
        }

        var m = new Matrix(
            matrix.M11 / sx, matrix.M12 / sx, matrix.M13 / sx, 0.0f,
            matrix.M21 / sy, matrix.M22 / sy, matrix.M23 / sy, 0.0f,
            matrix.M31 / sz, matrix.M32 / sz, matrix.M33 / sz, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);

        rotation = Quaternion.CreateFromRotationMatrix(m);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix CreateFromVector3(Vector3 row1, Vector3 row2, Vector3 row3, Vector3 row4)
    {
        var v1 = new Vector4(row1, 0);
        var v2 = new Vector4(row2, 0);
        var v3 = new Vector4(row3, 0);
        var v4 = new Vector4(row4, 0);

        return new Matrix(v1, v2, v3, v4);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix CreateFromRotScalePos(Vector3 rotation, Vector3 scale, Vector3 position)
    {
        var _worldMatrix = Matrix.Identity;
        _worldMatrix *= Matrix.CreateScale(scale);
        _worldMatrix *= Matrix.CreateFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);
        _worldMatrix *= Matrix.CreateTranslation(position);
        return _worldMatrix;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix CreateFromRotScalePos(Quaternion rotation, Vector3 scale, Vector3 position)
    {
        var _worldMatrix = Matrix.Identity;
        _worldMatrix *= Matrix.CreateScale(scale);
        _worldMatrix *= Matrix.CreateFromQuaternion(rotation);
        _worldMatrix *= Matrix.CreateTranslation(position);
        return _worldMatrix;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Matrix FromBepuMatrix(in BepuUtilities.Matrix matrix)
    {
        return new Matrix(matrix.X, matrix.Y, matrix.Z, matrix.W);
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static BoundingSphere TranslateBoundingSphere(BoundingSphere boundingSphere, in System.Numerics.Matrix4x4 matrix)
    {
        System.Numerics.Vector3 center = new System.Numerics.Vector3(boundingSphere.Center.X, boundingSphere.Center.Y, boundingSphere.Center.Z);
        matrix.TransPosRef(ref center);
        boundingSphere.Center = center;
        matrix.ExtractScale(out var scale);
        boundingSphere.Radius *= MathF.Max(scale.X, MathF.Max(scale.Y, scale.Z));
        return boundingSphere;
    }

    //
    // 摘要:
    //     Transforms the unit X direction using a quaternion.
    //
    // 参数:
    //   rotation:
    //     Rotation to apply to the vector.
    //
    //   result:
    //     Transformed vector.
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransformUnitX(this Quaternion rotation, out Vector3 result)
    {
        float num = rotation.Y + rotation.Y;
        float num2 = rotation.Z + rotation.Z;
        float num3 = rotation.X * num;
        float num4 = rotation.X * num2;
        float num5 = rotation.Y * num;
        float num6 = rotation.Z * num2;
        float num7 = rotation.W * num;
        float num8 = rotation.W * num2;
        result.X = 1f - num5 - num6;
        result.Y = num3 + num8;
        result.Z = num4 - num7;
    }

    //
    // 摘要:
    //     Transforms the unit Y vector using a quaternion.
    //
    // 参数:
    //   rotation:
    //     Rotation to apply to the vector.
    //
    //   result:
    //     Transformed vector.
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransformUnitY(this Quaternion rotation, out Vector3 result)
    {
        float num = rotation.X + rotation.X;
        float num2 = rotation.Y + rotation.Y;
        float num3 = rotation.Z + rotation.Z;
        float num4 = rotation.X * num;
        float num5 = rotation.X * num2;
        float num6 = rotation.Y * num3;
        float num7 = rotation.Z * num3;
        float num8 = rotation.W * num;
        float num9 = rotation.W * num3;
        result.X = num5 - num9;
        result.Y = 1f - num4 - num7;
        result.Z = num6 + num8;
    }

    //
    // 摘要:
    //     Transforms the unit Z vector using a quaternion.
    //
    // 参数:
    //   rotation:
    //     Rotation to apply to the vector.
    //
    //   result:
    //     Transformed vector.
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static void TransformUnitZ(this Quaternion rotation, out Vector3 result)
    {
        float num = rotation.X + rotation.X;
        float num2 = rotation.Y + rotation.Y;
        float num3 = rotation.Z + rotation.Z;
        float num4 = rotation.X * num;
        float num5 = rotation.X * num3;
        float num6 = rotation.Y * num2;
        float num7 = rotation.Y * num3;
        float num8 = rotation.W * num;
        float num9 = rotation.W * num2;
        result.X = num5 + num9;
        result.Y = num7 - num8;
        result.Z = 1f - num4 - num6;
    }

}
